#include <assert.h>
#include "imageconvert.h"


void debayer_GBRG(const image_u8_t * raw,  image_u32_t * out)
{
    assert(raw->width == out->width && raw->height == out->height);
    assert(raw->stride != 0);

    for (uint32_t y = 0; y < raw->height; y+=2 ) {
        for (uint32_t x = 0; x < raw->width; x+=2) {
            int32_t r = 0, g = 0, b = 0;

            // compute indices into bayer pattern for the nine 2x2 blocks we'll use.
            int32_t X00 = (y-2)*raw->width + (x-2);
            int32_t X01 = (y-2)*raw->width + (x+0);
            int32_t X02 = (y-2)*raw->width + (x+2);
            int32_t X10 = (y+0)*raw->width + (x-2);
            int32_t X11 = (y+0)*raw->width + (x+0);
            int32_t X12 = (y+0)*raw->width + (x+2);
            int32_t X20 = (y+2)*raw->width + (x-2);
            int32_t X21 = (y+2)*raw->width + (x+0);
            int32_t X22 = (y+2)*raw->width + (x+2);

            // handle the edges of the screen.
            if (y < 2) {
                X00 += 2*raw->width;
                X01 += 2*raw->width;
                X02 += 2*raw->width;
            }
            if (y+2 >= raw->height) {
                X20 -= 2*raw->width;
                X21 -= 2*raw->width;
                X22 -= 2*raw->width;
            }
            if (x < 2) {
                X00 += 2;
                X10 += 2;
                X20 += 2;
            }
            if (x+2 >= raw->width) {
                X02 -= 2;
                X12 -= 2;
                X22 -= 2;
            }

            // XXX Performance for C port -- remove 0xff
            // top left pixel (G)
            r = (raw->buf[X01+raw->width] + raw->buf[X11+raw->width]) / 2;
            g = raw->buf[X11];
            b = ((raw->buf[X10+1]) + raw->buf[X11+1]) / 2;
            out->buf[y*out->width+x] = (r<<16) | (g<<8) | b;

            // top right pixel (B)
            r = (raw->buf[X01+raw->width] + raw->buf[X02 + raw->width] + raw->buf[X01 + raw->width]  +  raw->buf[X12 + raw->width]) / 4;
            g = (raw->buf[X01 + raw->width + 1] + raw->buf[X11] + raw->buf[X12] + raw->buf[X11 + raw->width + 1]) / 4;
            b = raw->buf[X11 + 1];
            out->buf[y*out->width + x + 1] = (r<<16) | (g<<8) | b;

            // bottom left pixel (R)
            r = raw->buf[X11 + raw->width];
            g = (raw->buf[X11] + raw->buf[X10 + raw->width + 1] + raw->buf[X11 + raw->width + 1] + raw->buf[X21]) / 4;
            b = (raw->buf[X10 + 1] + raw->buf[X11 + 1] + raw->buf[X20 + 1] + raw->buf[X21 + 1]) / 4;
            out->buf[y*out->width + out->width + x] = (r<<16) | (g<<8) | b;

            // bottom right pixel (G)
            r = (raw->buf[X11 + raw->width] + raw->buf[X12 + raw->width]) / 2;
            g = raw->buf[X11 + raw->width + 1];
            b = (raw->buf[X11 + 1] + raw->buf[X21 + 1]) / 2;
            out->buf[y*out->width + out->width + x + 1] = (r<<16) | (g<<8) | b;
        }
    }

}

void debayer_GBRG_half(const image_u8_t * raw,  image_u32_t * out)
{
    assert(raw->width == out->width*2 && raw->height == out->height*2);
    assert(raw->stride != 0);

    for (uint32_t y = 0; y < out->height; y++ ) {
        for (uint32_t x = 0; x < out->width; x++ ) {
            uint32_t ix = x*2;
            uint32_t iy = y*2;

            uint32_t g1 = raw->buf[(iy + 0)*raw->width + ix + 0];
            uint32_t b1 = raw->buf[(iy + 0)*raw->width + ix + 1];
            uint32_t r1 = raw->buf[(iy + 1)*raw->width + ix + 0];
            uint32_t g2 = raw->buf[(iy + 1)*raw->width + ix + 1];

            out->buf[y*out->stride + x + 0] = (r1<<16) | (((g1 + g2)/2)<<8) | (b1);
        }
    }
}
